/*
 * @file base_server.h
 * @brief	base class with all the controls for the trunk/gripper.
 *
 * @author Michiel Hegemans
 * @author Peter Evers
 *
 * @date Jan 2013
 */

#ifndef BASE_SERVER_H_
#define BASE_SERVER_H_

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float32.h>

#include "comm/gripper_msg.h"
#include "comm/trunk_msg.h"
#include "topic/topic.h"

class BaseServer {
	/*
	 * VARIABLES
	 */
protected:
	ros::NodeHandle nh_;
private:
	int _trunk_state;
	int _gripper_state;
	int _gripper_sensor;
	geometry_msgs::Twist _wheels_speed;

	ros::Subscriber _trunk_sub;				/*!< Listen to trunk status. */
	ros::Subscriber _wheels_sub;			/*!< Listen to wheels status (velocity). */
	ros::Subscriber _gripper_sub;			/*!< Listen to gripper status. */

	ros::Publisher _trunk_pub;				/*!< Publish trunk status. */
	ros::Publisher _wheels_speed_pub;		/*!< Publish wheels speed status. */
	ros::Publisher _wheels_linear_pub;		/*!< Publish wheels linear position status. */
	ros::Publisher _wheels_angular_pub;		/*!< Publish wheels angular position status. */
	ros::Publisher _gripper_pub;			/*!< Publish gripper status. */

	std::string _action_name;
public:
	/*
	 * FUNCTIONS
	 */
private:
	void trunkCallback(const comm::trunk::ConstPtr &msg);
	void wheelsCallback(const geometry_msgs::Twist::ConstPtr &msg);
	void gripperCallback(const comm::gripper::ConstPtr &msg);
	void sensorCallback(const std_msgs::Float32::ConstPtr &msg);
protected:
	void publishTrunk(comm::trunk msg);
	void publishWheelsSpeed(geometry_msgs::Twist msg);
	void publishWheelsLinear(std_msgs::Float32 msg);
	void publishWheelsAngular(std_msgs::Float32 msg);
	void publishGripper(comm::gripper msg);

	int getTrunkState();
	int getGripperState();
	int getSensorDistance();
	geometry_msgs::Twist getWheelsSpeed();
public:
	BaseServer(std::string name) :
		nh_("~"),
		_action_name(name)
	{
		_trunk_pub = nh_.advertise<comm::trunk>(Topic::MOTOR_TRUNK, 10);
		_wheels_speed_pub = nh_.advertise<geometry_msgs::Twist>(Topic::NAV_VELOCITY, 10);
		_wheels_linear_pub = nh_.advertise<std_msgs::Float32>(Topic::NAV_LINEAR, 10);
		_wheels_angular_pub = nh_.advertise<std_msgs::Float32>(Topic::NAV_ANGULAR, 10);
		_gripper_pub = nh_.advertise<comm::gripper>(Topic::MOTOR_GRIPPER, 10);

		_gripper_sub = nh_.subscribe(Topic::SENSOR_GRIPPER, 10, &BaseServer::sensorCallback, this);
		_trunk_sub = nh_.subscribe(Topic::TRUNK_STATUS, 10, &BaseServer::trunkCallback, this);
		_wheels_sub = nh_.subscribe(Topic::WHEELS_STATUS, 10, &BaseServer::wheelsCallback, this);
		_gripper_sub = nh_.subscribe(Topic::GRIPPER_STATUS, 10, &BaseServer::gripperCallback, this);
	}
	virtual ~BaseServer() {
		nh_.shutdown();
	}
};


#endif /* BASE_SERVER_H_ */
